#include "cpurepursuit.h"

const char fileName[] = "/home/shukebeita/data/data.txt";

CPurePursuit::CPurePursuit(): v(0), heading(0), Kv(0.2), Kp(0.8), Ld0(3),
    dt(0.05), L(2.91), targetSpeed(10), idxTarget(0)
{
    //初始位置
    pos = {0, 2};
    // 读位置文件，获取位置信息
    posRef = readPosPoints(XRef, YRef);

    posActual.push_back(pos);
    headingActual.push_back(heading);
    vActual.push_back(v);
}

CPurePursuit::~CPurePursuit(){

}

inline double CPurePursuit::normFunc(double posRefX, double posRefY, double posX, double posY){
    return sqrt(pow(posRefX - posX, 2) + pow(posRefY - posY, 2));
}

vector<vector<double>> CPurePursuit::readPosPoints(vector<double> &XRef, vector<double> &YRef){
    ifstream readFile;
    readFile.open(fileName);

    if(!readFile.is_open()){
        cout << "file failed !!!" << endl;
        exit(0); //表示程序正常退出
    }

    string dataBuff;
    while(getline(readFile, dataBuff)){
        //cout << dataBuff << endl;
        //cout << dataBuff.length() << endl;
        string tempData;
        vector<string> tempStr;
        for(int i = 0; i <= dataBuff.length(); i ++){
            if(dataBuff[i] == ',' || dataBuff[i] == '\0'){
                //cout << tempData << endl;
                tempStr.push_back(tempData);
                tempData.clear();
            }else{
                tempData += dataBuff[i];
            }
        }
        //cout << tempStr[0] << tempStr[1] << tempStr[2] << endl;
        //cout << stod(tempStr[0]) << ',' << stod(tempStr[1])  << ',' << stod(tempStr[2]) << endl;
        XRef.push_back(stod(tempStr[0]));
        YRef.push_back(stod(tempStr[1]));
        //YawRef.push_back(stod(tempStr[2]));

    }

    readFile.close();

    vector<vector<double>> res(2, vector<double>(XRef.size(), 0));
    res[0] = XRef;
    res[1] = YRef;

    // 画出曲线，这里作为验证
    //plt::plot(XRef, YRef, "r-");
    //plt::show();

    return res;
}

look_ahead_t CPurePursuit::findLookAheadPoint(vector<double> pos, double v,
                                              vector<vector<double>> posRef, double Kv, double Ld0){
    //找到距离当前位置最近的一个参考轨迹点的序号
    int lenOfPosRef = posRef[0].size();

    vector<double> dist;
    for(int i = 0; i < lenOfPosRef; i++){
        double normRes  = normFunc(posRef[0][i], posRef[1][i], pos[0], pos[1]);
        //cout << "normRes = " << normRes << endl;
        dist.push_back(normRes);
    }

    int idx = min_element(dist.begin(), dist.end()) - dist.begin();

    //从该点开始向轨迹前方搜索，找到与预瞄距离最相近的一个轨迹点
    double Lsteps = 0; // 参考轨迹上几个相邻点的累计距离
    double Ld = Kv*v + Ld0; // Ld0 是预瞄距离的下限值

    while(Lsteps < Ld && idx < lenOfPosRef-1){
        Lsteps = Lsteps + normFunc(posRef[0][idx+1], posRef[1][idx+1], posRef[0][idx], posRef[1][idx]);
        //cout << "Lsteps = " << Lsteps << endl;
        idx ++;
    }
    //cout << "idx = " << idx << endl;
    look_ahead_t res;
    res.idxTarget = idx;
    vector<double> tempLookAheadPoint = {posRef[0][idx], posRef[1][idx]};
    res.lookAheadPoint = tempLookAheadPoint;
    //cout << "res.lookAheadPoint[0] = " << res.lookAheadPoint[0] << "res.lookAheadPoint[1] = " << res.lookAheadPoint[1] << endl;
    return res;
}

double CPurePursuit::purePursuit(vector<double> lookAheadPoint, int idxTarget,
                   vector<double> pos, double heading, double v,
                   vector<vector<double>> posRef, double Kv, double Ld0, double L){
    int lenOfPosRef = posRef[0].size();
    vector<double> tempPoint;
    if(idxTarget < lenOfPosRef-1)
        tempPoint = lookAheadPoint;
    else{
        tempPoint = {posRef[0][lenOfPosRef-1], posRef[1][lenOfPosRef-1]};
    }

    double alpha_ = atan2(tempPoint[1] - pos[1], tempPoint[0] - pos[0]) - heading;
    double Ld_ = Kv*v + Ld0;

    //前轮转角
    double delta_ = atan2(2*L*sin(alpha_), Ld_);

    return delta_;
}

new_state_t CPurePursuit::updataState(double a, vector<double> pos,
                                      double heading, double v, double delta,
                                      double L, double dt){
    vector<double> newPos(2);
    newPos[0] = pos[0] + v*cos(heading)*dt;
    newPos[1] = pos[1] + v*sin(heading)*dt;
    double newHeading = heading + v*dt*tan(delta)/L;
    double newV = v + a*dt;

    new_state_t res;
    res.pos = newPos;
    res.heading = newHeading;
    res.v = newV;

    return res;
}

void CPurePursuit::plotRes(vector<double> XRef, vector<double> YRef, vector<double> XActual, vector<double> YActual){
    vector<double> plotXActual;
    vector<double> plotYActual;
    for(int i = 0; i < XActual.size(); i++) {
        plotXActual.push_back(XActual[i]);
        plotYActual.push_back(YActual[i]);
        if (i % 10 == 0) {
            plt::clf();
            plt::named_plot("refPos", XRef, YRef, "r-");
            plt::named_plot("posActual", plotXActual, plotYActual, "b*");

            // Set x-axis to interval [0,1000000]
            plt::xlim(-10, 210);

            // Add graph title
            plt::title("Pure Pursuit");
            plt::xlabel("X");
            plt::ylabel("Y");

            // Enable legend.
            plt::legend();
            // Display plot continuously
            plt::pause(0.2);
        }
    }
    plt::show();
}
